/**
 * @file:          Foc.c
 * @brief:
 * @details:
 * @author:        wjh
 * @date created:  2023.06.14
 * @version:       1.0.0
 * @par copyright (c):
 *
 * @par history (desc):
 *   version:1.0.0, wjh, 2023.06.14,02:48:38
 */

/* Include Files **************************************************************/
#include "Foc.h"
#include "base_math.h"

/* Global Variable Define *****************************************************/

/* Function Define ************************************************************/
uint8 Foc_Init(struct Foc *self)
{
    self->Clark = Foc_Clark;
    self->IClark = Foc_IClark;
    self->Park = Foc_Park;
    self->IPark = Foc_IPark;
    self->SetClarkMode = Foc_SetClarkMode;

    self->SetClarkMode(self, self->param_clark_mode);

    return TRUE;
}

void Foc_Clark(float32 A, float32 B, float32 C, float32 *alpha, float32 *beta)
{
    *alpha = A - 0.5 * B - 0.5 * C;
    *beta = 0.866025 * B - 0.866025 * C;
}

void Foc_IClark(float32 alpha, float32 beta, float32 *A, float32 *B, float32 *C)
{
    *A = alpha;
    *B = -0.5 * alpha + 0.866025 * beta;
    *C = -0.5 * alpha - 0.866025 * beta;
}

void Foc_Park(float32 alpha, float32 beta, float32 theta, float32 *d, float32 *q)
{
    // float32 sin_theta = sinf(theta); 
    // float32 cos_theta = cosf(theta); 
    float32 sin_theta = sin_bm(theta); 
    float32 cos_theta = cos_bm(theta); 
    
    *d = alpha * cos_theta + beta * sin_theta;
    *q = alpha * -sin_theta + beta * cos_theta;

    // *d = alpha * cos_bm(theta) + beta * sin_bm(theta);
    // *q = alpha * -sin_bm(theta) + beta * cos_bm(theta);

}

void Foc_IPark(float32 d, float32 q, float32 theta, float32 *alpha, float32 *beta)
{
    // float32 sin_theta = sinf(theta); 
    // float32 cos_theta = cosf(theta); 
    float32 sin_theta = sin_bm(theta); 
    float32 cos_theta = cos_bm(theta); 

    *alpha = d * cos_theta - q * sin_theta;
    *beta = d * sin_theta + q * cos_theta;

    // *alpha = d * cos_bm(theta) - q * sin_bm(theta);
    // *beta = d * sin_bm(theta) + q * cos_bm(theta);
}

void Foc_SetClarkMode(Foc *self, enum CLARK_MODE clark_mode)
{
    self->param_clark_mode = clark_mode;

    switch (self->param_clark_mode)
    {
    case CLARK_MODE_EQUAL_AMPLITUDE_TRANSFORM:
        self->k_clark = 2 / 3;
        break;

    case CLARK_MODE_EQUAL_POWER_TRANSFORM:
        self->k_clark = 0.816497;
        break;
    default:
        break;
    }
}